#include "PhysicSystem.h"
#include "PhysicComponents.h"
#include "PositionComponent.h"
#include "Base/Exceptions.h"
#include "Base/Logger.h"
#include "MathUtiles.h"
#include <boost/lexical_cast.hpp>
#include <string>

PhysicSystem::PhysicSystem():mpt_map(150,150)
{

}
void PhysicSystem::update(float dt)
{
	// ON RECUPERE LES ENTITES DE LA GRID DE L'ENTITYMANAGER ET CELLES DU PHYSICSYSTEM
	sf::Vector2f updatePos = mpt_app->getView().getCenter();
	std::deque<int> gridEM = entityManager->getGrid()->getNeighbourhoodAt(0,updatePos);
	
	// ON DELETE LES DEUX NEIGHBOURHOODS
	entityManager->getGrid()->deleteNeighbourhood(0,updatePos);
	
    // UPDATE DES DYNAMICS EN FONCTION DE LA DERNIERE GRID DU PHYSICSYSTEM
	for(std::deque<int>::iterator itr = gridEM.begin(); itr!=gridEM.end(); itr++)
    {
		if(entityManager->hasEntity((*itr)))
		{
			if(mpt_entitiesID.find(*itr)!=mpt_entitiesID.end())
			{
				BPL::EntityPtr tempEnt(entityManager->getEntity(*itr));
				DynamicComponentPtr physicEnt = tempEnt->getAs<DynamicComponent>();
				PositionComponentPtr posEnt = tempEnt->getAs<PositionComponent>();
			
				// CALCUL DE LA DIRECTION INITIALE
				if(dt>0.04)
					dt = 0.04;
				sf::Vector2f moveVec = dt*physicEnt->moveDirection;
				physicEnt->collide = false;
				physicEnt->collideXWithNoSlope = false;
				physicEnt->collideYWithNoSlope = false;
				physicEnt->normalCollision = sf::Vector2f(0.0f,0.0f);
				sf::Vector2f sizeBox = physicEnt->sizeBox;
				physicEnt->moveDirection = sf::Vector2f(0.0f,0.0f);	
				int sizeTile = entityManager->sizeTile;

				// PUIS LE TEST AVEC LA STATIC
				if(moveVec.x > 0)
				{
					sf::Vector2f posCorner(posEnt->m_position.x + moveVec.x + sizeBox.x - posEnt->m_center.x, posEnt->m_position.y - posEnt->m_center.y);
					sf::Vector2f posCorner2(posEnt->m_position.x + moveVec.x + sizeBox.x - posEnt->m_center.x, posEnt->m_position.y+sizeBox.y-1 - posEnt->m_center.y);
					
					sf::Vector2i posMap((posEnt->m_position.x + moveVec.x + sizeBox.x - posEnt->m_center.x)/sizeTile, (posEnt->m_position.y - posEnt->m_center.y)/sizeTile);
					sf::Vector2i posMap2((posEnt->m_position.x + moveVec.x + sizeBox.x - posEnt->m_center.x)/sizeTile, (posEnt->m_position.y+sizeBox.y-1 - posEnt->m_center.y)/sizeTile);

					bool collision1 = pointCollideTile(posCorner);
					bool collision2 = pointCollideTile(posCorner2);
					
					if(!collision1 && !collision2)
					{
						posEnt->m_position.x+=moveVec.x;
					}
					else
					{
						sf::Vector2i posTile((posEnt->m_position.x + sizeBox.x/2 - posEnt->m_center.x)/sizeTile, (posEnt->m_position.y + moveVec.y + sizeBox.y - 1 - posEnt->m_center.y)/sizeTile);
						int entS = mpt_map.getEntity(posTile);
						if(entS != 0 && entityManager->getEntity(entS)->getAs<StaticComponent>()->typeStatic == StaticComponent::SLOPELEFT
						&& !collision1)
						{
							posEnt->m_position.x+=moveVec.x;
						}
						else
						{
							posEnt->m_position.x = posMap.x * sizeTile - sizeBox.x + posEnt->m_center.x;
							physicEnt->collide = true;
							physicEnt->collideXWithNoSlope = true;
							physicEnt->normalCollision += sf::Vector2f(-1.0f, 0.0f);
							if(collision1)
								physicEnt->collideWith = mpt_map.getEntity(posMap);
							else
								physicEnt->collideWith = mpt_map.getEntity(posMap2);
						}
					}
				}
				else if(moveVec.x < 0)
				{
					sf::Vector2f posCorner(posEnt->m_position.x + moveVec.x - posEnt->m_center.x, posEnt->m_position.y - posEnt->m_center.y);
					sf::Vector2f posCorner2(posEnt->m_position.x + moveVec.x - posEnt->m_center.x, posEnt->m_position.y+sizeBox.y-1 - posEnt->m_center.y);
					
					sf::Vector2i posMap((posEnt->m_position.x + moveVec.x - posEnt->m_center.x)/sizeTile, (posEnt->m_position.y - posEnt->m_center.y)/sizeTile);
					sf::Vector2i posMap2((posEnt->m_position.x + moveVec.x - posEnt->m_center.x)/sizeTile, (posEnt->m_position.y+sizeBox.y-1 - posEnt->m_center.y)/sizeTile);

					bool collision1 = pointCollideTile(posCorner);
					bool collision2 = pointCollideTile(posCorner2);
					
					if(!collision1 && !collision2)
					{
						posEnt->m_position.x+=moveVec.x;
					}
					else
					{
						sf::Vector2i posTile((posEnt->m_position.x + sizeBox.x/2 - posEnt->m_center.x)/sizeTile, (posEnt->m_position.y + moveVec.y + sizeBox.y - 1 - posEnt->m_center.y)/sizeTile);
						int entS = mpt_map.getEntity(posTile);
						if(entS != 0 && entityManager->getEntity(entS)->getAs<StaticComponent>()->typeStatic == StaticComponent::SLOPERIGHT
						&& !collision1)
						{
							posEnt->m_position.x+=moveVec.x;
						}
						else
						{
							posEnt->m_position.x = (posMap.x+1) * sizeTile + posEnt->m_center.x;
							physicEnt->collide = true;
							physicEnt->collideXWithNoSlope = true;
							physicEnt->normalCollision += sf::Vector2f(1.0f, 0.0f);
							if(collision1)
								physicEnt->collideWith = mpt_map.getEntity(posMap);
							else
								physicEnt->collideWith = mpt_map.getEntity(posMap2);

						}
					}
				}

				if(moveVec.y > 0)
				{
					sf::Vector2f posCorner(posEnt->m_position.x - posEnt->m_center.x, posEnt->m_position.y + moveVec.y + sizeBox.y - posEnt->m_center.y);
					sf::Vector2f posCorner2(posEnt->m_position.x+sizeBox.x-1 - posEnt->m_center.x, posEnt->m_position.y + moveVec.y + sizeBox.y - posEnt->m_center.y);
					
					sf::Vector2i posMap((posEnt->m_position.x - posEnt->m_center.x)/sizeTile, (posEnt->m_position.y + moveVec.y + sizeBox.y - posEnt->m_center.y)/sizeTile);
					sf::Vector2i posMap2((posEnt->m_position.x+sizeBox.x-1 - posEnt->m_center.x)/sizeTile, (posEnt->m_position.y + moveVec.y + sizeBox.y - posEnt->m_center.y)/sizeTile);
					
					bool collision1 = pointCollideTile(posCorner);
					bool collision2 = pointCollideTile(posCorner2);
					
					if(!collision1 && !collision2)
					{
						posEnt->m_position.y+=moveVec.y;
						if(physicEnt->type == JUMPING)
						{
							if(!(boost::dynamic_pointer_cast<JumpingComponent>(physicEnt)->jump))
							{
								boost::dynamic_pointer_cast<JumpingComponent>(physicEnt)->jump = true;
								boost::dynamic_pointer_cast<JumpingComponent>(physicEnt)->jumpState = 0.25;
							}
						}
					}
					else
					{
						sf::Vector2i posTile((posEnt->m_position.x + sizeBox.x/2 - posEnt->m_center.x)/sizeTile, (posEnt->m_position.y + moveVec.y + sizeBox.y - 1 - posEnt->m_center.y)/sizeTile);
						int entS = mpt_map.getEntity(posTile);
						if(entS != 0 && entityManager->getEntity(entS)->getAs<StaticComponent>()->typeStatic == StaticComponent::SLOPERIGHT
						|| entS != 0 && entityManager->getEntity(entS)->getAs<StaticComponent>()->typeStatic == StaticComponent::SLOPELEFT
						)
						{
							posEnt->m_position.y+=moveVec.y;
							if(physicEnt->type == JUMPING)
							{
								if(!(boost::dynamic_pointer_cast<JumpingComponent>(physicEnt)->jump))
								{
									boost::dynamic_pointer_cast<JumpingComponent>(physicEnt)->jump = true;
									boost::dynamic_pointer_cast<JumpingComponent>(physicEnt)->jumpState = 0.25;
								}
							}
						}
						else
						{
							posEnt->m_position.y = posMap.y * sizeTile - sizeBox.y + posEnt->m_center.y;	
							physicEnt->collide = true;
							physicEnt->collideYWithNoSlope = true;
							physicEnt->normalCollision += sf::Vector2f(0.0f, -1.0f);
							if(collision1)
								physicEnt->collideWith = mpt_map.getEntity(posMap);
							else
								physicEnt->collideWith = mpt_map.getEntity(posMap2);

							if(physicEnt->type == JUMPING)
								boost::dynamic_pointer_cast<JumpingComponent>(physicEnt)->jump = false;
						}
					}
				}
				else if(moveVec.y < 0)
				{
					sf::Vector2f posCorner(posEnt->m_position.x - posEnt->m_center.x, posEnt->m_position.y + moveVec.y - posEnt->m_center.y);
					sf::Vector2f posCorner2(posEnt->m_position.x+sizeBox.x-1 - posEnt->m_center.x, posEnt->m_position.y + moveVec.y - posEnt->m_center.y);
					
					sf::Vector2i posMap((posEnt->m_position.x - posEnt->m_center.x)/sizeTile, (posEnt->m_position.y + moveVec.y - posEnt->m_center.y)/sizeTile);
					sf::Vector2i posMap2((posEnt->m_position.x+sizeBox.x-1 - posEnt->m_center.x)/sizeTile, (posEnt->m_position.y + moveVec.y - posEnt->m_center.y)/sizeTile);
					
					bool collision1 = pointCollideTile(posCorner);
					bool collision2 = pointCollideTile(posCorner2);
					
					if(!collision1 && !collision2)
					{
						posEnt->m_position.y+=moveVec.y;
					}
					else
					{				
						posEnt->m_position.y = (posMap.y+1) * sizeTile + posEnt->m_center.y;
						physicEnt->collide = true;
						physicEnt->collideYWithNoSlope = true;
						physicEnt->normalCollision += sf::Vector2f(0.0f, 1.0f);
						if(collision1)
							physicEnt->collideWith = mpt_map.getEntity(posMap);
						else
							physicEnt->collideWith = mpt_map.getEntity(posMap2);

						if(physicEnt->type == JUMPING)
							boost::dynamic_pointer_cast<JumpingComponent>(physicEnt)->jumpState = 0;
					}
				}
				sf::Vector2i posTile((posEnt->m_position.x + sizeBox.x/2 - posEnt->m_center.x)/sizeTile, (posEnt->m_position.y + sizeBox.y - 1 - posEnt->m_center.y)/sizeTile);
				sf::Vector2f point = posEnt->m_position + sf::Vector2f(sizeBox.x/2 - posEnt->m_center.x, sizeBox.y - posEnt->m_center.y);
				sf::Vector2f relativePos(point.x - sizeTile * posTile.x,point.y - sizeTile * posTile.y);
				int entS = mpt_map.getEntity(posTile);
				if(entS != 0)
				{
					StaticComponentPtr physS = entityManager->getEntity(entS)->getAs<StaticComponent>();
					if(physS->typeStatic == StaticComponent::SLOPERIGHT
					&& abs(relativePos.y)>abs(relativePos.x))
					{
						posEnt->m_position.y = posTile.y * sizeTile + relativePos.x - sizeBox.y + posEnt->m_center.y;
						physicEnt->collide = true;
						physicEnt->normalCollision += sf::Vector2f(1.0f, -1.0f);
						physicEnt->collideWith = entS;

						if(physicEnt->type == JUMPING)
							boost::dynamic_pointer_cast<JumpingComponent>(physicEnt)->jump = false;
					}
					else if(physS->typeStatic == StaticComponent::SLOPELEFT
					&& abs(relativePos.y)>sizeTile-abs(relativePos.x))
					{
						posEnt->m_position.y = posTile.y * sizeTile + (sizeTile - relativePos.x) - sizeBox.y + posEnt->m_center.y;
						physicEnt->collide = true;
						physicEnt->normalCollision += sf::Vector2f(-1.0f, -1.0f);
						physicEnt->collideWith = entS;
						if(physicEnt->type == JUMPING)
							boost::dynamic_pointer_cast<JumpingComponent>(physicEnt)->jump = false;
					}
				}
				physicEnt->normalCollision = normalize(physicEnt->normalCollision);
			}
			entityManager->getGrid()->addEntity(0,*itr,entityManager->getEntity(*itr)->getAs<PositionComponent>()->m_position);
		}
		else
		{
#ifdef DEFINE_MODE
			std::string str = "BUG PHYSIC : ";
			str += boost::lexical_cast<std::string>(*itr);
			BPL::Logger::log(str.c_str());
#endif
			deleteEntity(*itr);
		}
		

    }

	// UPDATE DE LA GRID
}

EntityMap* PhysicSystem::getMap()
{
	return &mpt_map;
}

void PhysicSystem::addEntity(int ent)
{
	if(!entityManager->hasEntity(ent))
		return;

    mpt_entitiesID.insert(ent);
	entityManager->getEntity(ent)->addSystem(this);
	PhysicComponentPtr compPhy = entityManager->getEntity(ent)->getAs<PhysicComponent>();
	PositionComponentPtr compPos = entityManager->getEntity(ent)->getAs<PositionComponent>();

	if(compPhy->type==STATIC)
	{
		int sizeTile = entityManager->sizeTile;
		mpt_map.addEntity(ent, sf::Vector2i(compPos->m_position.x/sizeTile,compPos->m_position.y/sizeTile));
	}
}

void PhysicSystem::deleteEntity(int ent)
{
	std::set<int>::iterator itr = mpt_entitiesID.find(ent);
	if(itr != mpt_entitiesID.end())
	{
		mpt_entitiesID.erase(itr);
		mpt_map.deleteEntity(ent);
#ifdef DEFINE_MODE
		BPL::Logger::log("delete from PhysicSystem");
#endif
	}
	else
	{
#ifdef DEFINE_MODE
		std::string str = "BUG DELETE PHYSIC : ";
		str += boost::lexical_cast<std::string>(ent);
		BPL::Logger::log(str.c_str());
#endif
	}
}

bool PhysicSystem::pointCollideTile(const sf::Vector2f& point, bool slope)
{
	int sizeTile = entityManager->sizeTile;
	sf::Vector2i posTile(point.x/sizeTile, point.y/sizeTile);
	int ent = mpt_map.getEntity(posTile);
	if(ent==0 || !entityManager->hasEntity(ent))
		return false;
	else
	{
		StaticComponentPtr phys = entityManager->getEntity(ent)->getAs<StaticComponent>();
		if(phys->typeStatic==StaticComponent::FULL)
			return true;
		else if(phys->typeStatic==StaticComponent::SLOPELEFT && slope
			 || phys->typeStatic==StaticComponent::SLOPERIGHT && slope)
		{
			return true;
		}
		else
			return false;
	}
}